/*
 * Copyright (c) 2015-2016, Thomas Keh
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
 
#include "entry_publisher.h"
#include "utils.h"
#include "logger.h"
#include "rclcpp/rclcpp.hpp"
#include "sdo_error.h"

#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/u_int16.hpp>
#include <std_msgs/msg/u_int32.hpp>
#include <std_msgs/msg/u_int64.hpp>
#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/int16.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/int64.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/float64.hpp>

#include <string>

namespace kaco {

EntryPublisher::EntryPublisher(Device& device, const std::string& busname, const std::string& entry_name, const ReadAccessMethod access_method)
	: m_device(device), m_entry_name(entry_name), m_access_method(access_method)
{

	uint8_t node_id = device.get_node_id();
	m_device_prefix = busname + "/device" + std::to_string(node_id) + "/";
	// no spaces and '-' allowed in ros names
	m_name = Utils::escape(entry_name);
	m_type = device.get_entry_type(entry_name);
}

void EntryPublisher::advertise(std::shared_ptr<rclcpp::Node> node) {

	std::string topic = m_device_prefix+"get_"+m_name;
	DEBUG_LOG("Advertising "<<topic);

	switch(m_type) {
		case Type::uint8:
			m_publisher = node->create_publisher<std_msgs::msg::UInt8>(topic, queue_size);
			break;
		case Type::uint16:
			m_publisher = node->create_publisher<std_msgs::msg::UInt16>(topic, queue_size);
			break;
		case Type::uint32:
			m_publisher = node->create_publisher<std_msgs::msg::UInt32>(topic, queue_size);
			break;
		case Type::uint64:
			m_publisher = node->create_publisher<std_msgs::msg::UInt64>(topic, queue_size);
			break;
		case Type::int8:
			m_publisher = node->create_publisher<std_msgs::msg::Int8>(topic, queue_size);
			break;
		case Type::int16:
			m_publisher = node->create_publisher<std_msgs::msg::Int16>(topic, queue_size);
			break;
		case Type::int32:
			m_publisher = node->create_publisher<std_msgs::msg::Int32>(topic, queue_size);
			break;
		case Type::int64:
			m_publisher = node->create_publisher<std_msgs::msg::Int64>(topic, queue_size);
			break;
		case Type::boolean:
			m_publisher = node->create_publisher<std_msgs::msg::Bool>(topic, queue_size);
			break;
		case Type::string:
			m_publisher = node->create_publisher<std_msgs::msg::String>(topic, queue_size);
			break;
		default:
			ERROR("[EntryPublisher::advertise] Invalid entry type.")
	}
}

void EntryPublisher::publish(std::shared_ptr<rclcpp::Node> node) {
	// PRINT("[EntryPublisher] publish: " << m_entry_name);
	try {
		// PRINT("[EntryPublisher] get entry name: " << m_entry_name);
		Value value = m_device.get_entry(m_entry_name, m_access_method);
		double timestamp = m_device.get_entry_timestamp(m_entry_name);
		if (timestamp < 0.1) {
			RCLCPP_INFO_THROTTLE(node->get_logger(), *(node->get_clock()), 500, 
				"[EntryPublisher] node[%d][%s] pdo data not received...", 
				(unsigned char)m_device.get_node_id(), m_entry_name.c_str());
			return;
		}

		switch(m_type) {
		    case Type::uint8: {
		        auto publisher = std::static_pointer_cast<rclcpp::Publisher<std_msgs::msg::UInt8>>(m_publisher);
		        if (publisher) {
		            std_msgs::msg::UInt8 msg;
		            msg.data = static_cast<uint8_t>(value);
		            publisher->publish(msg);
		        } else {
		            RCLCPP_ERROR(node->get_logger(), "Invalid publisher type for uint8");
		        }
		        break;
		    }
		    case Type::uint16: {
		        auto publisher = std::static_pointer_cast<rclcpp::Publisher<std_msgs::msg::UInt16>>(m_publisher);
		        if (publisher) {
		            std_msgs::msg::UInt16 msg;
		            msg.data = static_cast<uint16_t>(value);
		            publisher->publish(msg);
		        } else {
		            RCLCPP_ERROR(node->get_logger(), "Invalid publisher type for uint16");
		        }
		        break;
		    }
		    case Type::uint32: {
		        auto publisher = std::static_pointer_cast<rclcpp::Publisher<std_msgs::msg::UInt32>>(m_publisher);
		        if (publisher) {
		            std_msgs::msg::UInt32 msg;
		            msg.data = static_cast<uint32_t>(value);
		            publisher->publish(msg);
		        } else {
		            RCLCPP_ERROR(node->get_logger(), "Invalid publisher type for uint32");
		        }
		        break;
		    }
		    case Type::uint64: {
		        auto publisher = std::static_pointer_cast<rclcpp::Publisher<std_msgs::msg::UInt64>>(m_publisher);
		        if (publisher) {
		            std_msgs::msg::UInt64 msg;
		            msg.data = static_cast<uint64_t>(value);
		            publisher->publish(msg);
		        } else {
		            RCLCPP_ERROR(node->get_logger(), "Invalid publisher type for uint64");
		        }
		        break;
		    }
		    case Type::int8: {
		        auto publisher = std::static_pointer_cast<rclcpp::Publisher<std_msgs::msg::Int8>>(m_publisher);
		        if (publisher) {
		            std_msgs::msg::Int8 msg;
		            msg.data = static_cast<int8_t>(value);
		            publisher->publish(msg);
		        } else {
		            RCLCPP_ERROR(node->get_logger(), "Invalid publisher type for int8");
		        }
		        break;
		    }
		    case Type::int16: {
		        auto publisher = std::static_pointer_cast<rclcpp::Publisher<std_msgs::msg::Int16>>(m_publisher);
		        if (publisher) {
		            std_msgs::msg::Int16 msg;
		            msg.data = static_cast<int16_t>(value);
		            publisher->publish(msg);
		        } else {
		            RCLCPP_ERROR(node->get_logger(), "Invalid publisher type for int16");
		        }
		        break;
		    }
		    case Type::int32: {
		        auto publisher = std::static_pointer_cast<rclcpp::Publisher<std_msgs::msg::Int32>>(m_publisher);
		        if (publisher) {
		            std_msgs::msg::Int32 msg;
		            msg.data = static_cast<int32_t>(value);
		            publisher->publish(msg);
		        } else {
		            RCLCPP_ERROR(node->get_logger(), "Invalid publisher type for int32");
		        }
		        break;
		    }
		    case Type::int64: {
		        auto publisher = std::static_pointer_cast<rclcpp::Publisher<std_msgs::msg::Int64>>(m_publisher);
		        if (publisher) {
		            std_msgs::msg::Int64 msg;
		            msg.data = static_cast<int64_t>(value);
		            publisher->publish(msg);
		        } else {
		            RCLCPP_ERROR(node->get_logger(), "Invalid publisher type for int64");
		        }
		        break;
		    }
		    case Type::boolean: {
		        auto publisher = std::static_pointer_cast<rclcpp::Publisher<std_msgs::msg::Bool>>(m_publisher);
		        if (publisher) {
		            std_msgs::msg::Bool msg;
		            msg.data = static_cast<bool>(value);
		            publisher->publish(msg);
		        } else {
		            RCLCPP_ERROR(node->get_logger(), "Invalid publisher type for boolean");
		        }
		        break;
		    }
		    case Type::string: {
		        auto publisher = std::static_pointer_cast<rclcpp::Publisher<std_msgs::msg::String>>(m_publisher);
		        if (publisher) {
		            std_msgs::msg::String msg;
		            msg.data = std::string(value);
		            publisher->publish(msg);
		        } else {
		            RCLCPP_ERROR(node->get_logger(), "Invalid publisher type for string");
		        }
		        break;
		    }
		    default: {
		        RCLCPP_ERROR(node->get_logger(), "Invalid entry type");
		        break;
		    }
		}
		
	} catch (const sdo_error& error) {
		// TODO: only catch timeouts?
		ERROR("Exception in EntryPublisher::publish(): "<<error.what());
	}
}


EntryTimestampPublisher::EntryTimestampPublisher(Device& device, const std::string& busname, const std::string& entry_name, const ReadAccessMethod access_method)
	: m_device(device), m_entry_name(entry_name), m_access_method(access_method)
{

	uint8_t node_id = device.get_node_id();
	m_device_prefix = busname + "/device" + std::to_string(node_id) + "/";
	// no spaces and '-' allowed in ros names
	m_name = Utils::escape(entry_name);
}

void EntryTimestampPublisher::advertise(std::shared_ptr<rclcpp::Node> node) {

	std::string topic = m_device_prefix+"get_"+m_name +"/timestamp";
	DEBUG_LOG("Advertising "<<topic);
	
	m_publisher = node->create_publisher<std_msgs::msg::Float64>(topic, queue_size);
}

void EntryTimestampPublisher::publish(std::shared_ptr<rclcpp::Node> node) {
	// PRINT("[EntryPublisher] publish: " << m_entry_name);
	try {
		// PRINT("[EntryPublisher] get entry name: " << m_entry_name);
		double value = m_device.get_entry_timestamp(m_entry_name);
		 auto publisher = std::static_pointer_cast<rclcpp::Publisher<std_msgs::msg::Float64>>(m_publisher);
        if (publisher) {
            std_msgs::msg::Float64 msg;
            msg.data = static_cast<double>(value);
            publisher->publish(msg);
        } else {
            RCLCPP_ERROR(node->get_logger(), "Invalid publisher type for float64");
        }	
	} catch (const sdo_error& error) {
		// TODO: only catch timeouts?
		ERROR("Exception in EntryPublisher::publish(): "<<error.what());
	}

}


} // end namespace kaco